import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    ld = LaunchDescription()

    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    fishbot_description_dir = get_package_share_directory('fishbot_description')
    fishbot_cartographer_dir = get_package_share_directory('fishbot_cartographer')
    fishbot_navigation2_dir = get_package_share_directory('fishbot_navigation2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')

    # Gazebo参数
    robot_name_in_model = 'fishbot'
    urdf_name = "gazebo.urdf"
    world_name = "fishbot.world"
    urdf_model_path = os.path.join(fishbot_description_dir, f'urdf/{urdf_name}')
    gazebo_world_path = os.path.join(fishbot_description_dir, f'world/{world_name}')

    # rviz2参数
    # rviz_config_dir = os.path.join(fishbot_navigation2_dir, 'rviz/default.rviz')
    rviz_config_dir = os.path.join(nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')

    # cartographer参数
    resolution = LaunchConfiguration('resolution', default='0.05')# 地图的分辨率  
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')# 地图的发布周期  
    configuration_directory = LaunchConfiguration('configuration_directory',default= os.path.join(fishbot_cartographer_dir, 'config') )# 配置文件夹路径  
    configuration_basename = LaunchConfiguration('configuration_basename', default='simulate.lua')# 配置文件

    # nav2、map参数
    map_name = "map_sim.yaml"
    map_yaml_path = os.path.join(fishbot_navigation2_dir, 'maps', map_name)
    nav2_param_path = LaunchConfiguration('params_file', default=os.path.join(fishbot_navigation2_dir, 'param', 'fishbot_nav2.yaml'))

    # 机器人状态发布
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        arguments=[urdf_model_path]
    )

    joint_state_publisher_node = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        name='joint_state_publisher_gui',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=[urdf_model_path]
    )
    # odom发布
    odom_node = Node(
        package='agv_control',
        executable='odom',
        output='screen'
    )

    # 定义 Gazebo 节点
    gazebo_node = ExecuteProcess(
        cmd=['ros2', 'launch', 'fishbot_description', 'gazebo.launch.py'],
        output='screen'
    )

    # 定义 雷达 节点
    radar_node = ExecuteProcess(
        cmd=['ros2', 'launch', 'sdkeli_ls_udp2',
             'sdkeli_ls1207de_udp_with_1_lidar_launch.py'],
        output='screen'
    )

    # 定义 IMU 节点
    imu_node = ExecuteProcess(
        cmd=['ros2', 'launch', 'fdilink_ahrs', 'ahrs_driver.launch.py'],
        output='screen'
    )

    # 定义 RViz2 节点
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config_dir],
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen')

    # 定义 Cartographer 节点
    cartographer_node = ExecuteProcess(
        cmd=['ros2', 'launch', 'fishbot_cartographer', 'cartographer.launch.py'],
        # output='screen'
    )

    mapserver_node = Node(
        package='nav2_map_server',
        executable='map_server',
        name='map_server',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time},
                    {'yaml_filename': map_yaml_path}]
    )
    lifecycle_node = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_mapper',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time},
                    {'autostart': True},
                    {'node_names': ['map_server']}]
    )

    # 定义 AGV控制 节点
    agv_control = ExecuteProcess(
        cmd=['ros2', 'run', 'agv_control', 'agv'],
        # output='screen'
    )
    # 定义 导航 节点
    nav2 = ExecuteProcess(
        cmd=['ros2', 'launch', 'fishbot_navigation2', 'navigation2.launch.py'],
        output='screen'
    )

    start_gazebo_cmd = ExecuteProcess(
        cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_factory.so', gazebo_world_path],
        output='screen')

    spawn_entity_cmd = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', robot_name_in_model,  '-file', urdf_model_path], output='screen')

    cartographer_node = Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        arguments=['-configuration_directory', configuration_directory,
                   '-configuration_basename', configuration_basename,
                #    '-initial_pose', '{to_trajectory_id=0, relative_pose={translation={x=10.0, y=2.0, z=0.0}, rotation={x=0.0, y=0.0, z=0.707, w=0.707}}}'
                #    '-load_state_filename',map_yaml_path
                   ])

    occupancy_grid_node = Node(
        package='cartographer_ros',
        executable='occupancy_grid_node',
        name='occupancy_grid_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
        # arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec, '-pure_localization',1])
        arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec])
    
    nav2_bringup_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
        launch_arguments={
            'map': map_yaml_path,
            'use_sim_time': use_sim_time,
            'params_file': nav2_param_path}.items(),
    )

    # 将所有节点添加到 LaunchDescription
    # ld.add_action(radar_node)
    
    ld.add_action(robot_state_publisher_node)
    ld.add_action(joint_state_publisher_node)
    ld.add_action(start_gazebo_cmd)
    ld.add_action(spawn_entity_cmd)
    ld.add_action(cartographer_node)
    ld.add_action(occupancy_grid_node)
    ld.add_action(nav2_bringup_launch)
    ld.add_action(rviz_node)
    # ld.add_action(gazebo_node)
    # ld.add_action(nav2)   
    # ld.add_action(radar_node)
    # ld.add_action(imu_node)
    # ld.add_action(odom_node)
    # ld.add_action(agv_control)
    
    # ld.add_action(mapserver_node)
    # ld.add_action(lifecycle_node)

    return ld
